//+--------------------------------------------------------------------
//| Copyright(c) 2012 - an.vuongngocduy					
//| This file is a part of Compact Game Engine (CGE)	
//+--------------------------------------------------------------------
#ifndef __CGE_QUATERNION_H__
#define __CGE_QUATERNION_H__

#include "cgeBasicTypes.h"
#include "cgeVector3.h"
#include "cgeMath.h"

namespace cge
{
	namespace std
	{
		class Quaternion
		{
		public:
			/// Default constructor
			Quaternion();

			/// Copy constructor
			Quaternion(const Quaternion& _quat);

			/// Construct Quaternion from 4 floats
			Quaternion(f32 x, f32 y, f32 z, f32 w);

			/// Destructor
			~Quaternion();

			/// Calculate Quaternion's magnitude
			f32 Magnitude() const;

			/// Normalize Quaternion
			Quaternion& Normalize();

			/// Normalize Quaternion
			Quaternion Normalize() const;

			/// Conjugation Quaternion
			Quaternion Conjugate() const;

			/// Do spherical linear interpolation
			Quaternion Slerp(const Quaternion& quat, f32 weight) const;

			/// Do linear interpolation
			Quaternion Lerp(const Quaternion& quat, f32 weight) const;

			/// Retrieve the axis which rotate about
			Vector3f GetAxis() const;

			/// Assign operator
			Quaternion& operator = (const Quaternion& quat);

			/// Sum of 2 Quaternion
			Quaternion operator + (const Quaternion& quat) const;

			/// Subtract of 2 Quaternion
			Quaternion operator - (const Quaternion& quat) const;

			/// Negate Quaternion
			Quaternion operator - (void) const;

			/// Multiply Quaternion by Quaternion
			Quaternion operator * (const Quaternion& quat) const;

			/// Multiply Quaternion by real value
			Quaternion operator * (f32 scalar) const;

			/// Divide Quaternion by a real value
			Quaternion operator / (f32 scalar) const;

			/// Compare 2 Quaternions are the same
			b8 operator == (const Quaternion& quat) const;

			/// Compare 2 Quaternions are different
			b8 operator != (const Quaternion& quat) const;

		public:
			union
			{
				struct { f32 X, Y, Z, W; };
				f32 XYZW[4];
			};
		};
	}

	// Quaternion class implementation
	namespace std
	{
		/*----------------------------------------------------------------------------------------*/
		inline Quaternion::Quaternion()
			: X(0), Y(0), Z(0), W(0)
		{}

		/*----------------------------------------------------------------------------------------*/
		inline Quaternion::Quaternion(const Quaternion& quat)
			 : X(quat.X), Y(quat.Y), Z(quat.Z), W(quat.W)
		{}

		/*----------------------------------------------------------------------------------------*/
		inline Quaternion::Quaternion(f32 x, f32 y, f32 z, f32 w)
			: X(x), Y(y), Z(z), W(w)
		{}

		/*----------------------------------------------------------------------------------------*/
		inline Quaternion::~Quaternion(){}

		/*----------------------------------------------------------------------------------------*/
		inline void Quaternion::Normalize()
		{
			f32 mag = Magnitude();
			if(mag != 1) return;
			X /= mag;	Y /= mag;	Z /= mag;	W /= mag;
		}

		/*----------------------------------------------------------------------------------------*/
		inline f32 Quaternion::Magnitude() const
		{
			return math::Math::Sqrt(X * X + Y * Y + Z * Z + W * W);
		}

		/*----------------------------------------------------------------------------------------*/
		inline Quaternion Quaternion::Conjugate() const
		{
			return Quaternion( -X, -Y, -Z, W);
		}

		/*----------------------------------------------------------------------------------------*/
		inline Quaternion Quaternion::Slerp(const Quaternion& quat, f32 weight) const
		{
			f32 cosTheta = X * quat.X + Y * quat.Y + Z * quat.Z + W * quat.W;
			Quaternion q1 = quat;
			if(cosTheta < 0)
			{
				q1 = -q1;
				cosTheta = -cosTheta;
			}

			if(cosTheta < 1.00001f && cosTheta > 0.99999)
			{
				return Lerp(q1, weight);
			}

			float theta = acosf(cosTheta);

			float a = sinf((1 - weight) * theta) / sinf(theta);
			float b = sinf(weight * theta) / sinf(theta);

			return Quaternion(X * a + q1.X * b, Y * a + q1.Y * b, Z * a + q1.Z * b, W * a + q1.W * b);
		}

		/*----------------------------------------------------------------------------------------*/
		inline Quaternion Quaternion::Lerp(const Quaternion& quat, f32 weight) const
		{
			Quaternion retQ( weight * quat.X + (1 - weight)*X,	
						weight * quat.Y + (1 - weight) * Y,
						weight * quat.Z + (1 - weight) * Z,	
						weight * quat.W + (1 - weight) * W);
			retQ.Normalize();
			return retQ;
		}

		/*----------------------------------------------------------------------------------------*/
		inline Vector3f Quaternion::GetAxis() const
		{
			f32 sinHalfAngle = 1 - (W * W);
			return Vector3f( X / sinHalfAngle, Y / sinHalfAngle, Z / sinHalfAngle);
		}

		/*----------------------------------------------------------------------------------------*/
		inline Quaternion& Quaternion::operator = (const Quaternion& quat)
		{
			X = quat.X;		Y = quat.Y;		Z = quat.Z;		W = quat.W;
			return *this;
		}

		/*----------------------------------------------------------------------------------------*/
		inline Quaternion Quaternion::operator + (const Quaternion& quat) const
		{
			return Quaternion( X + quat.X, Y + quat.Y, Z + quat.Z, W + quat.W);
		}

		/*----------------------------------------------------------------------------------------*/
		inline Quaternion Quaternion::operator - (const Quaternion& quat) const
		{
			return Quaternion( X - quat.X, Y - quat.Y, Z - quat.Z, W - quat.W);
		}

		/*----------------------------------------------------------------------------------------*/
		inline Quaternion Quaternion::operator - (void) const
		{
			return Quaternion( -X, -Y, -Z, -W);
		}

		/*----------------------------------------------------------------------------------------*/
		inline Quaternion Quaternion::operator * (const Quaternion& quat) const
		{
			Quaternion retQ(quat.W * X + W * quat.X + Y * quat.Z - Z * quat.Y,
							quat.W * Y + W * quat.Y + Z * quat.X - X * quat.Z,
							quat.W * Z + W * quat.Z + X * quat.Y - Y * quat.X,
							quat.W * W - X * quat.X - Y * quat.Y - Z * quat.Z);
			retQ.Normalize();
			return retQ;
		}

		/*----------------------------------------------------------------------------------------*/
		inline Quaternion Quaternion::operator * (f32 scalar) const
		{
			return Quaternion( scalar * X, scalar * Y, scalar * Z, scalar * W);
		}

		/*----------------------------------------------------------------------------------------*/
		inline Quaternion Quaternion::operator / (f32 scalar) const
		{
			return Quaternion( X / scalar, Y / scalar, Z / scalar, W / scalar);
		}
	}
}

#endif//__CGE_QUATERNION_H__